/**
 * @file obstacle.cpp
 * @author circleup (circleup@foxmial.com)
 * @brief 点云栅格化
 * @version 0.2
 * @date 2020-07-24
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#include "global.h"

#include "euler_kmeans++.h"
#include "screen_driver.h"

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>

#include <sensor_msgs/PointCloud2.h>
#include <ros/ros.h>


ros::Subscriber pointcloud_sub;
ros::Publisher rasterize_poiontcloud_pub;

void CloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg);

int main( int argc, char** argv ) {

  ros::init(argc, argv, "RASTERIZE");
  ros::NodeHandle n;
  ros::Rate loop_rate(10);

  pointcloud_sub = n.subscribe<sensor_msgs::PointCloud2>("/rfans_driver/rfans_points", 2, CloudHandler);
  rasterize_poiontcloud_pub = n.advertise<sensor_msgs::PointCloud2>("/notground_cloud", 2);

  loop_rate.sleep();
  ros::spin();
}

void CloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
  double time = ros::Time::now().toSec();
  pcl::PointCloud<pcl::PointXYZ> point_cloud_in;
  pcl::PointCloud<pcl::PointXYZ> point_cloud_ground;
  pcl::PointCloud<pcl::PointXYZ> point_cloud_not_ground;

  pcl::PointCloud<pcl::PointXYZ> point_cloud_in_filter;

  pcl::PointCloud<pcl::PointXYZRGB> point_cloud_cluster_out;

  sensor_msgs::PointCloud2 point_cloud_out;

  pcl::PointXYZ point;
  pcl::PointXYZRGB point_rgb;

  //体素化滤波
  pcl::fromROSMsg(*laserCloudMsg, point_cloud_in);
  pcl::VoxelGrid<pcl::PointXYZ> vg;
  vg.setInputCloud (point_cloud_in.makeShared());
  vg.setLeafSize (0.10f, 0.10f, 0.10f);
  vg.filter (point_cloud_in_filter);

  class euler_kmeans_plus2 cluster;

  for (uint16_t i = 0; i < point_cloud_in_filter.points.size(); i++)
  {
    point.x = point_cloud_in_filter.points[i].x;
    point.y = point_cloud_in_filter.points[i].y;
    point.z = point_cloud_in_filter.points[i].z;
    if (point.z < (-HEIGHT+0.1))
    {
      point_cloud_ground.points.push_back(point);
    }
    else
    {
      // point.z = 0;
      point_cloud_not_ground.points.push_back(point);
    }
  }

  cluster.SetPointCloudIn(point_cloud_not_ground.makeShared());
  cluster.PonitCloudNormalization();
  cluster.InitKClusterCenter();
  cluster.ClusterCenterIteration();

  // pcl::toROSMsg(point_cloud_not_ground, point_cloud_out);
  // point_cloud_out.header.frame_id = "car";
  // rasterize_poiontcloud_pub.publish(point_cloud_out);

  time = ros::Time::now().toSec() - time;
  ROS_INFO("time is %f, point_cloud_in_filter size is %d", time, point_cloud_in_filter.points.size());
}